#include <logger.h>

#include "modbustransfer_p.h"
#include "dx_type.h"
#include "iot_debug.h"
#include "crc16.h"
#include "tools/m_sleep.h"


#define GET_INT16_FROM_INT8(tab_int8, index) ((tab_int8[(index)] << 8) + tab_int8[(index) + 1])


ModbusTransferPrivate::ModbusTransferPrivate(IPhyLink *dev, int mode) :
    dev_(dev),
    mode_(mode),
    com_settings_(dev->comSettings())
{
}

ModbusTransferPrivate::~ModbusTransferPrivate()
{
}

int ModbusTransferPrivate::sendCmdAndRecv(const ModbusCmdInfo &cmd, uint8_t *recv_buf)
{
    uint8_t tx_buf[16];
    uint8_t pos = encode(cmd, tx_buf, 16);
    uint32_t retryed = 0;
reTry:
    int err_code = readAndWrite(tx_buf, pos, recv_buf, cmd.expect_recv_size);
    if (err_code != ERR_NO_ERROR && err_code != ERR_SLAVE_RET_ERR) {
        if (retryed < com_settings_->retry_cnt_at_recv_timeout) {
            dev_->clear();
            retryed++;
            log_info("[%s_%d] found_err:\"%s\"  retry:%d --\n", dev_->UartName(),
                     tx_buf[mode_==ModbusTransfer::TCP?6:0], DXErrorString(err_code), retryed);
            goto reTry;
        }
    }
    return err_code;
}

int ModbusTransferPrivate::setYKCmd(const ModbusYKCmdInfo &cmd)
{
    uint8_t tx_buf[16];
    uint8_t rx_buf[16];
    uint8_t pos = encode(cmd, tx_buf, 16);
    uint8_t recv_size = pos;
    if (cmd.func_code == 0x10)
        recv_size -= 3;

    uint32_t retryed = 0;
    const ComSettings *settings = dev_->comSettings();
reTry:
    int err_code = readAndWrite(tx_buf, pos, rx_buf, recv_size);
    if (err_code != ERR_NO_ERROR && err_code != ERR_SLAVE_FUNC_ERR)
    {
        if (retryed < settings->retry_cnt_at_recv_timeout) {
            retryed++;
            goto reTry;
        }
    }
    if (err_code == ERR_SLAVE_FUNC_ERR)
    {
        log_error("error::  modbus info->[addr %02X func %02X regaddr %02X%02X]\n", rx_buf[0], rx_buf[1], tx_buf[2], tx_buf[3]);
    }

    return err_code;
}

uint8_t ModbusTransferPrivate::encode(const ModbusCmdInfo &cmd, uint8_t *buf, uint8_t buf_size)
{
    uint8_t pos = 0;

    if (mode_ == ModbusTransfer::TCP)
    {
        if (buf_size < 12)
            return 0;
        // tid
        buf[pos++] = cmd.order >> 8;
        buf[pos++] = cmd.order & 0x00ff;
        // portocol modbus
        buf[pos++] = 0;
        buf[pos++] = 0;
        // lenth
        buf[pos++] = 0;
        buf[pos++] = 6;
    }
    else
    {
        if (buf_size < 8)
            return 0;
    }

    buf[pos++] = cmd.dev_addr;
    buf[pos++] = cmd.func_code;
    buf[pos++] = cmd.reg_addr >> 8;
    buf[pos++] = cmd.reg_addr  & 0x00ff;
    buf[pos++] = cmd.reg_num  >> 8;
    buf[pos++] = cmd.reg_num   & 0x00ff;

    if (mode_ == ModbusTransfer::RTU)
    {
        uint16_t crc = calcCRC16(buf, pos);
        buf[pos++] = crc  & 0x00ff;
        buf[pos++] = crc >> 8;
    }
    else if (mode_ == ModbusTransfer::RTU_CRC_HL)
    {
        uint16_t crc = calcCRC16(buf, pos);
        buf[pos++] = crc >> 8;
        buf[pos++] = crc  & 0x00ff;
    }

    return pos;
}

uint8_t ModbusTransferPrivate::encode(const ModbusYKCmdInfo &cmd, uint8_t *buf, uint8_t buf_size)
{
    uint8_t pos = 0;

    if (mode_ == ModbusTransfer::TCP)
    {
        if (buf_size < 12)
            return 0;
        // tid
        buf[pos++] = cmd.order >> 8;
        buf[pos++] = cmd.order & 0x00ff;
        // portocol modbus
        buf[pos++] = 0;
        buf[pos++] = 0;
        // lenth
        buf[pos++] = 0;
        buf[pos++] = 6;
    }
    else
    {
        if (buf_size < 8)
            return 0;
    }

    buf[pos++] = cmd.dev_addr;
    buf[pos++] = cmd.func_code;
    buf[pos++] = cmd.reg_addr >> 8;
    buf[pos++] = cmd.reg_addr  & 0x00ff;

    if (cmd.func_code == 0x10)
    {
        buf[pos++] = 0x00;
        buf[pos++] = 0x01;  // reg num
        buf[pos++] = 0x02;  // byte num
    }

    buf[pos++] = cmd.status  >> 8;
    buf[pos++] = cmd.status   & 0x00ff;

    if (mode_ == ModbusTransfer::RTU)
    {
        uint16_t crc = calcCRC16(buf, pos);
        buf[pos++] = crc  & 0x00ff;
        buf[pos++] = crc >> 8;
    }
    else if (mode_ == ModbusTransfer::RTU_CRC_HL)
    {
        uint16_t crc = calcCRC16(buf, pos);
        buf[pos++] = crc >> 8;
        buf[pos++] = crc  & 0x00ff;
    }

    return pos;
}

int ModbusTransferPrivate::parseRTU(const uint8_t *rd, uint16_t rl, uint8_t slave_id, uint8_t func)
{
    if ( (slave_id != rd[0]) || (func != (rd[1]&0x7F)) )
        return ERR_RECV_DATA_ERROR;
    if (rd[1]&0x80) {
        if (rl < 5)
            return ERR_RECV_DATA_ERROR;
        if (verifyCRC(rd, 5))
            return ERR_NO_ERROR;
        else
            return ERR_SLAVE_RET_ERR;
    }
    else {
        if (verifyCRC(rd, rl))
            return ERR_NO_ERROR;
        else
            return ERR_RECV_DATA_ERROR;
    }
}

bool ModbusTransferPrivate::verifyCRC(const uint8_t *rx_data, uint16_t rx_len)
{
    int rx_crc = calcCRC16(rx_data, rx_len-2);

    if (mode_ != ModbusTransfer::RTU_CRC_HL)
        return rx_crc == (rx_data[rx_len-2]+(rx_data[rx_len-1]<<8));
    else
        return rx_crc == (rx_data[rx_len-1]+(rx_data[rx_len-2]<<8));
}

int ModbusTransferPrivate::recvRTU(uint8_t slave_id, uint8_t func, uint8_t *rd,
                                   uint16_t rl, uint16_t &pos)
{
    uint32_t n = dev_->read(rd, 2, com_settings_->recv_frame_timeout_ms);
    pos = static_cast<uint16_t>(n);
    if (n != 2) {
        return ERR_RECV_TIMEOUT;
    }
    if ( (slave_id != rd[0]) || (func != (rd[1]&0x7F)) )
        return ERR_RECV_DATA_ERROR;
    if (rd[1]&0x80) {
        n = dev_->read(rd+2, 3, com_settings_->recv_char_timeout_ms);
        pos += static_cast<uint16_t>(n);
        if (n != 3)
            return ERR_RECV_TIMEOUT;
        if (verifyCRC(rd, 5))
            return ERR_SLAVE_FUNC_ERR;       //数据检验正常,执行也是出错,功能码最高位置1表示出错.
        else
            return ERR_SLAVE_RET_ERR;
    }
    else {
        n = dev_->read(rd+2, rl-2, com_settings_->recv_char_timeout_ms);
        pos += static_cast<uint16_t>(n);
        if (n+2 != rl)
            return ERR_RECV_TIMEOUT;
        if (verifyCRC(rd, rl))
            return ERR_NO_ERROR;
        else
            return ERR_RECV_DATA_ERROR;
    }
}

int ModbusTransferPrivate::recvTCP(const uint8_t *td, uint16_t tl, uint8_t *rd, uint16_t rl, uint16_t &pos)
{
    const ComSettings * settings = dev_->comSettings();

    uint32_t n = dev_->read(rd, 4, settings->recv_frame_timeout_ms);
    pos = static_cast<uint16_t>(n);
    if (n != 4)
        return ERR_RECV_TIMEOUT;
    if (memcmp(td, rd, 4) != 0)
        return ERR_RECV_DATA_ERROR;

    n = dev_->read(rd+4, 2, settings->recv_char_timeout_ms);
    pos += static_cast<uint16_t>(n);
    if (n != 2)
        return ERR_RECV_TIMEOUT;
    uint16_t len = static_cast<uint16_t>(GET_INT16_FROM_INT8(rd, 4)+6);
    if (rl != len) {
        if (len == 9) {     // slave return err
            n = dev_->read(rd+6, 3, settings->recv_char_timeout_ms);
            pos += static_cast<uint16_t>(n);
            if (n!=3)
                return ERR_RECV_TIMEOUT;
            if ( (td[6] != rd[6]) || ((td[7]|0x80) != rd[7]))
                return ERR_RECV_DATA_ERROR;
            return ERR_SLAVE_RET_ERR;
        }
        return ERR_RECV_DATA_ERROR;
    }

    n = dev_->read(rd+6, 2, settings->recv_char_timeout_ms);
    pos += static_cast<uint16_t>(n);
    if (n != 2)
        return ERR_RECV_TIMEOUT;
    if ( (td[6] != rd[6]) || (td[7] != (rd[7]&0x7F)) )
        return ERR_RECV_DATA_ERROR;

    if (rd[7]&0x80) {
        n = dev_->read(rd+8, 1, settings->recv_char_timeout_ms);
        pos += static_cast<uint16_t>(n);
        if (n != 1)
            return ERR_RECV_TIMEOUT;
        return ERR_SLAVE_RET_ERR;
    }
    else {
        n = dev_->read(rd+8, rl-8, settings->recv_char_timeout_ms);
        pos += static_cast<uint16_t>(n);
        if (n+8 != rl)
            return ERR_RECV_TIMEOUT;
        return ERR_NO_ERROR;
    }
}

int ModbusTransferPrivate::readAndWrite(const uint8_t *td, uint16_t tl, uint8_t *rd, uint16_t rl)
{
    const ComSettings * settings = dev_->comSettings();

    if (settings->dump_msg) {
        printf("[TX %d,%s] ", tl, logTime().c_str());
        for (int i=0; i<tl; i++) {
            printf("%02X ", td[i]);
        }
        printf("\n");
    }

    dev_->lock();
    if (tl != dev_->write(td, tl)) {
        dev_->unlock();
        return ERR_SEND_FAILED;
    }
    int err_code = ERR_NO_ERROR;
    uint16_t n = 0;

    if (mode_ == ModbusTransfer::TCP)
        err_code = recvTCP(td, tl, rd, rl, n);
    else
        err_code = recvRTU(td[0], td[1], rd, rl, n);

    if (err_code == ERR_RECV_DATA_ERROR || err_code == ERR_CRC_CHECK_FAILED)
    {
        if (settings->dump_msg)
        {
            printf("<RX error,%s> ", logTime().c_str());
            for (uint16_t i=0; i<n; i++) {
                printf("%02X ", rd[i]);
            }
        }
        int err = ERR_NO_ERROR;
        do {
            n = 0;
            err = continueRecvData(rd, rl, n);
            if (settings->dump_msg) {
                for (uint16_t i=0; i<n; i++) {
                    printf("%02X ", rd[i]);
                }
            }
        } while(n && err!=ERR_RECV_TIMEOUT);
        if (settings->dump_msg)
            printf("\n");
        dev_->unlock();
    }
    else {
        dev_->unlock();
        if (settings->dump_msg) {
            printf("<RX %d,%s> ", n, logTime().c_str());
            for (uint16_t i=0; i<n; i++) {
                printf("%02X ", rd[i]);
            }
            printf("\n");
        }
    }
    return err_code;
}

int ModbusTransferPrivate::continueRecvData(uint8_t *rd, uint16_t rl, uint16_t &pos)
{
    uint32_t n = dev_->read(rd, rl, com_settings_->recv_char_timeout_ms);
    pos += static_cast<uint16_t>(n);
    return n!=rl ? ERR_RECV_TIMEOUT : ERR_NO_ERROR;
}
